/*
 * @Author: Jiagang Chen
 * @Date: 2021-12-05 09:56:42
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-07 09:17:54
 * @Description: ...
 * @Reference: ...
 */

#pragma once

#include "lci_slam/vilens/common_header.h"

struct PointToPlaneImuFactor {
    static constexpr int NumResiduals() { return 1; }

    PointToPlaneImuFactor(const Eigen::Vector3d &reference,
                          const Eigen::Vector3d &target,
                          const Eigen::Vector3d &reference_normal,
                          double weight = 1.0)
        : reference_(reference),
          target_(target),
          reference_normal_(reference_normal),
          weight_(weight) {}

    template <typename T>
    bool operator()(const T *const pose_params, const T *const relpose_params,
                    T *residual) const {
        Eigen::Map<Eigen::Quaternion<T>> quat_lidar2imu(
            const_cast<T *>(relpose_params + 3));
        Eigen::Matrix<T, 3, 1> transformed_imu =
            quat_lidar2imu * target_.template cast<T>();
        transformed_imu(0, 0) += relpose_params[0];
        transformed_imu(1, 0) += relpose_params[1];
        transformed_imu(2, 0) += relpose_params[2];

        // rot_params and trans_params --> from imu to world
        Eigen::Map<Eigen::Quaternion<T>> quat(const_cast<T *>(pose_params + 3));
        Eigen::Matrix<T, 3, 1> transformed = quat * transformed_imu;
        transformed(0, 0) += pose_params[0];
        transformed(1, 0) += pose_params[1];
        transformed(2, 0) += pose_params[2];

        residual[0] =
            weight_ *
            (reference_.template cast<T>() - transformed).transpose() *
            reference_normal_.template cast<T>();
        return true;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Eigen::Vector3d reference_;
    Eigen::Vector3d target_;
    Eigen::Vector3d reference_normal_;
    double weight_ = 1.0;
};
